package project.robot;

import java.util.ArrayList;
import java.util.Calendar;

import javax.swing.text.StyleContext.SmallAttributeSet;

import com.sun.corba.se.impl.orbutil.closure.Constant;
import com.sun.xml.internal.bind.v2.runtime.reflect.opt.Const;

import project.common.*;
import project.map.OccupancyGridMap;
import project.map.Position;
import project.robot.detection.UltrasonicDetectionSystem;
import project.robot.detection.UltrasonicListener;
import project.robot.navigation.NavigationSystem;
import project.robot.pathplanning.BfsPathPlanner;
import project.robot.pathplanning.PathPlanningSystem;

import lejos.nxt.SensorPort;


/**
 * Aca se toman las decisiones y se actualiza el mapa. 
 * Se computa la posicion actual se decidde para donde moverse 
 * o para donde sensar, aca se piensa.
 *	
 */
public class Robot implements UltrasonicListener {
	private NavigationSystem navigator;
	private UltrasonicDetectionSystem usDetector;
	private PathPlanningSystem pathFinder;
	private OccupancyGridMap internalMap;
	private Position currentPosition;	
	private boolean isTurning=false;	//true cuando gira
	private boolean isMoving =false;	//true cuando avanza
	private int normalSpeed=90;
	private ArrayList<Position> unvisitedFrontier = new ArrayList<Position>(); //Lista de las celdas frontera que aun no se han visitado
	private ArrayList<Position> visitedPositions = new ArrayList<Position>(); //Celdas que ya visite.
	private int confidenceValue = 0;
	private short lastMovement=0;
	private int ultrasonicMaxRange=80;
	private boolean pause = false;

	public Robot (int ultrasonicMaxRange, int ultrasonicScanDelay)
	{
		this.ultrasonicMaxRange = ultrasonicMaxRange;

		usDetector = new UltrasonicDetectionSystem(SensorPort.S4, ultrasonicMaxRange, ultrasonicScanDelay);

		usDetector.addListener(this);

		pathFinder = new BfsPathPlanner();
	}

	@Override
	public void rangeReadingObtained(float reading) {

		int rangeReading = (int)reading + 7; //TODO: estos 7 centimetros que le sumo son para compenzar el hecho que el sensor no esta en el centro y yo considero para los calculos que si lo esta, lo que podria hacer para mejorarlo es aluna funcion que antes de hacer los calculos compute la posicion xy real del sensor y no el centro de la celda.

		if (this.isMoving)
			rangeReading += Math.round(this.navigator.getTraveledDistance()); //si me estoy moviendo a la lectura del sensor le tengo que restar lo que me movi  para que no me pinte cualquier celda.

		if (isTurning)
			currentPosition.angle = currentPosition.getOrientationAngle() + navigator.getAngleRotated(); //Si esta rotando calculo el angulo sumando lo que voy rotando
		else
			currentPosition.angle = currentPosition.getOrientationAngle(); 	//si no esta rotando (esta quieto) uso el angulo que corresponde con la orientacion que tengo.

		HimmMapUpdater.UpdateMap(this.internalMap, rangeReading, currentPosition,this.visitedPositions,this.ultrasonicMaxRange);
	}

	public void startMapping(int confidenceValue)
	{	
		this.confidenceValue = confidenceValue;

		long startTime = Calendar.getInstance().getTimeInMillis();

		mapear();

		long elapseTime = Calendar.getInstance().getTimeInMillis() - startTime;

		System.out.println("Fin del Mapeo. Tiempo total: " + elapseTime / 60000 + " minutos");
		this.internalMap.paint();
	}

	private void mapear()
	{
		Position goal= new Position();
		boolean goalReached = false;
		boolean continueMapping = true;
		int turnDirection = 1;

		startDetection();
		turn(350);

		while (continueMapping)
		{
			updateFrontier();

			if (this.unvisitedFrontier.contains(goal)) 	//si sigue siendo frontera. veo que hay por aca. sino me voy a buscar otra frontera.
			{
				turn(350 * turnDirection);
				updateFrontier();	
			}

			goalReached =false;
			while ((this.unvisitedFrontier.size()>0) && goalReached == false) //este bucle busca en la frontera hasta que puede llegar a alguna de sus celdas.
			{
				goal = getNearestFrontierCell();

				System.out.println("Ir a: " + goal.toString());

				ArrayList<Integer> pathToGoal = pathFinder.searchPathTo(this.currentPosition, goal, this.internalMap);

				goalReached = (pathToGoal.size()>0) ? executePath(pathToGoal) : false;
			}

			continueMapping = goalReached; //se finaliza el mapeo si no hay frontera accesible.
			turnDirection =  turnDirection * -1; //rotar una vez para cada lado.
		}

		stopDetection();
	}

	public void navigateTo(Position goal)
	{
		ArrayList<Integer> pathToGoal = pathFinder.searchPathTo(this.currentPosition, goal, this.internalMap);

		if (pathToGoal.size()>0)
			executePath(pathToGoal);
	}

	/*
	 * Actualiza el listado de celdas que componen la frontera.
	 */
	public void updateFrontier()
	{
		unvisitedFrontier.clear();

		for (int x = 0; x < internalMap.getMaxX(); x++) {
			for (int y = 0; y < internalMap.getMaxY(); y++) {
				if (internalMap.getValue(x, y) <= internalMap.getFreeCellValue() || (this.currentPosition.x==x && this.currentPosition.y==y) )
				{
					//si es una celda libre entonces ver si es frontera. 
					for (int i = x-1; i <= x+1; i++) {
						for (int j = y-1; j <= y+1; j++) {
							if ((internalMap.getValue(i, j)<= this.confidenceValue && internalMap.getValue(i, j) > internalMap.getFreeCellValue())
									|| (internalMap.getValue(i, j)==internalMap.getUnknownCellValue()))
							{
								Position p = new Position(x,y);
								if (!unvisitedFrontier.contains(p))
									unvisitedFrontier.add(p);
							}
						} 
					}
				}
			}
		}
	}

	private Position getNearestFrontierCell(){

		double menorDistancia = Double.MAX_VALUE;
		double distanciaActual;
		Position retorno;

		ArrayList<Position> frontierToWork;

		frontierToWork = this.unvisitedFrontier;

		int minimo=0;

		for (int i = 0; i < frontierToWork.size(); i++) {
			distanciaActual = pathFinder.searchPathTo(this.currentPosition, frontierToWork.get(i), this.internalMap).size();

			if (distanciaActual < menorDistancia)
			{
				menorDistancia = distanciaActual;
				minimo = i;
			}
		}

		retorno = frontierToWork.get(minimo);
		frontierToWork.remove(minimo);

		return retorno;
	}

	//Ejecutar las acciones planificadas
	public boolean executePath(ArrayList<Integer> path)
	{
		int i = 0;
		boolean pathFree = true;
		Integer action;

		while (i< path.size() && pathFree)
		{
			if (!isPause())
			{			
				action = path.get(i);

				switch (action) {
				case Constants.NORTH:
					orientToNorth();
					pathFree= this.internalMap.isNorthFree(this.currentPosition);
					break;
				case Constants.SOUTH:
					orientToSouth();
					pathFree= this.internalMap.isSouthFree(this.currentPosition);
					break;
				case Constants.EAST:
					orientToEast();
					pathFree= this.internalMap.isEastFree(this.currentPosition);
					break;
				case Constants.WEST:
					orientToWest();
					pathFree= this.internalMap.isWestFree(this.currentPosition);
					break;
				default:
					break;
				}

				if (pathFree)
					goForward(this.internalMap.getCellSize(), this.normalSpeed);

				i++;
			}
		}

		return pathFree;
	}

	public void orientToNorth()
	{
		switch (currentPosition.o) {
		case Constants.EAST:
			turn(88);
			break;

		case Constants.SOUTH:
			turn(170);
			break;

		case Constants.WEST:
			turn(-88);
			break;
		}
	}

	public void orientToSouth()
	{
		switch (currentPosition.o) {
		case Constants.NORTH:
			turn(170);
			break;

		case Constants.EAST:
			turn(-88);
			break;

		case Constants.WEST:
			turn(88);
			break;
		}
	}

	public void orientToEast()
	{
		switch (currentPosition.o) {
		case Constants.NORTH:
			turn(-88);
			break;

		case Constants.SOUTH:
			turn(88);
			break;

		case Constants.WEST:
			turn(170);
			break;
		}
	}

	public void orientToWest()
	{
		switch (currentPosition.o) {
		case Constants.NORTH:
			turn(88);
			break;

		case Constants.SOUTH:
			turn(-88);
			break;

		case Constants.EAST:
			turn(170);
			break;
		}
	}


	/*
	 * PRIMITIVAS DE MOVIMIENTO
	 */
	public void goForward(int range, int speed)
	{
		this.isMoving=true;

		if (this.lastMovement==Constants.FORWARD)
			speed= speed + speed/4;

		if (navigator.goForward(range-1, speed))
			updateCurrentPosition(range);

		this.isMoving=false;

		this.lastMovement=Constants.FORWARD;
	}

	public void turn(int angle)
	{
		this.isTurning=true;

		navigator.turn(angle);
		updateCurrentOrientation(angle);

		this.isTurning=false;

		this.lastMovement=Constants.TURN;
	}

	/**
	 * 
	 * ACTUALIZAR MAPA
	 * 
	 * */

	/**
	 * Actualiza la nueva posicion del robot luego de moverse
	 * @param distancia que se movio el robot
	 */
	private void updateCurrentPosition(int distancia){

		//libero la posicion actual
		internalMap.setValue(currentPosition.x,currentPosition.y,internalMap.getFreeCellValue());

		//moverme
		switch (currentPosition.o) {
		case Constants.NORTH:
			currentPosition.y+=distancia/internalMap.getCellSize();
			break;
		case Constants.EAST:
			currentPosition.x+=distancia/internalMap.getCellSize();
			break;
		case Constants.SOUTH:
			currentPosition.y-=distancia/internalMap.getCellSize();			
			break;
		case Constants.WEST:
			currentPosition.x-=distancia/internalMap.getCellSize();
			break;

		default:
			break;
		}

		internalMap.setValue(currentPosition.x, currentPosition.y,internalMap.getCeldaActual()+currentPosition.o);

		addVisitedPosition();
	}

	private void addVisitedPosition()
	{
		if (!visitedPositions.contains(currentPosition))
			visitedPositions.add(new Position(currentPosition.x,currentPosition.y));
	}

	/**
	 * Actualiza la nueva orientacion del robot luego de que gira
	 * 
	 * @param angulo que giro
	 */
	private void updateCurrentOrientation(int angulo)
	{
		switch (currentPosition.o) {
		case 1:
			if (angulo == -88)
				currentPosition.o=Constants.EAST;

			if (angulo == 88)
				currentPosition.o=Constants.WEST;

			if (angulo == 170)
				currentPosition.o=Constants.SOUTH;
			break;

		case 2:
			if (angulo == -88)
				currentPosition.o=Constants.SOUTH;
			if (angulo == 88)
				currentPosition.o=Constants.NORTH;	
			if (angulo == 170)
				currentPosition.o=Constants.WEST;
			break;

		case 3:
			if (angulo == -88)
				currentPosition.o=Constants.WEST;
			if (angulo == 88)
				currentPosition.o=Constants.EAST;
			if (angulo == 170)
				currentPosition.o=Constants.NORTH;	
			break;

		case 4:
			if (angulo == -88)
				currentPosition.o=Constants.NORTH;	
			if (angulo == 88)
				currentPosition.o=Constants.SOUTH;
			if (angulo == 170)
				currentPosition.o=Constants.EAST;
			break;

		default:
			break;
		}

		//actualizar con la nueva posicion a la que me movi.
		internalMap.setValue(currentPosition.x, currentPosition.y,internalMap.getCeldaActual()+currentPosition.o);
	}


	/** 
	 * Getters & Setters
	 * 
	 *   
	 *   
	 *   */
	public void setNavegacion(NavigationSystem navegacion) {
		this.navigator = navegacion;
	}
	public OccupancyGridMap getMapa() {
		return internalMap;
	}
	public void setMapa(OccupancyGridMap mapa) {
		this.internalMap = mapa;
	}
	public Position getPosActual() {
		return currentPosition;
	}
	public void setPosActual(Position posActual) {
		this.currentPosition = posActual;

		switch (this.currentPosition.o ) {
		case Constants.NORTH:
			this.currentPosition.angle = 88;
			break;

		case Constants.EAST:
			this.currentPosition.angle = 360;
			break;

		case Constants.SOUTH:
			this.currentPosition.angle = 270;
			break;

		case Constants.WEST:
			this.currentPosition.angle = 180;
			break;

		default:
			break;
		}

		internalMap.setValue(posActual.x, posActual.y,internalMap.getCeldaActual()+posActual.o);
		addVisitedPosition(); 		
	}

	public void setVelNormal(int velNormal) {
		this.normalSpeed = velNormal;
	}

	public void setUltrasonicMaxRange(short ultrasonicMaxRange) {
		this.ultrasonicMaxRange = ultrasonicMaxRange;
	}

	//TEST *********** Borrar
	public void startDetection()
	{
		usDetector.startDetection();
	}
	public void stopDetection()
	{
		usDetector.stopDetection();
	}

	private boolean isPause() {
		return pause;
	}

	public void setPause(boolean pause) {
		this.pause = pause;
	}
}

